/*
 * get_ik_circle.cpp
 *
 *  Created on: Aug 5, 2011
 *      Author: zhaofei
 */

#include <map>
#include <math.h>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <tf/transform_broadcaster.h>


class GetIKCircle{

public:

	GetIKCircle()
	{
		init();
	}

	bool init()
	{
		ros::service::waitForService("arm_kinematics/get_ik_solver_info");
		ros::service::waitForService("arm_kinematics/get_ik");

		query_client = root_handle_.serviceClient <kinematics_msgs::GetKinematicSolverInfo> ("arm_kinematics/get_ik_solver_info");
		ik_client = root_handle_.serviceClient < kinematics_msgs::GetPositionIK> ("arm_kinematics/get_ik");

		joint_state_publisher_ = root_handle_.advertise <sensor_msgs::JointState> ("joint_states", 1);

		if (query_client.call(request, response))
		{
			for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
			{
				ROS_DEBUG("Joint: %d %s", i, response.kinematic_solver_info.joint_names[i].c_str());
			}
		}
		else
		{
			ROS_ERROR("Could not call query service");
			ros::shutdown();
			exit(1);
		}

		gpik_req.timeout = ros::Duration(5.0);
		gpik_req.ik_request.ik_link_name = "left_arm_link_7";
		gpik_req.ik_request.pose_stamped.header.frame_id = "arm_reference_link";


		return true;
	}

	bool computeIK(double px, double py, double pz, double ox, double oy, double oz)
	{
		tf::Quaternion q = tf::createQuaternionFromRPY(ox,oy,oz);

		gpik_req.ik_request.pose_stamped.pose.position.x = px;
		gpik_req.ik_request.pose_stamped.pose.position.y = py;
		gpik_req.ik_request.pose_stamped.pose.position.z = pz;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = q.getX();
		gpik_req.ik_request.pose_stamped.pose.orientation.y = q.getY();
		gpik_req.ik_request.pose_stamped.pose.orientation.z = q.getZ();
		gpik_req.ik_request.pose_stamped.pose.orientation.w = q.getW();

		gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
		gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;

		for (unsigned int i = 0; i < response.kinematic_solver_info.joint_names.size(); i++)
		{
			gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position
																		 + response.kinematic_solver_info.limits[i].max_position) / 2.0;
		}

		if (ik_client.call(gpik_req, gpik_res))
		{
			if (gpik_res.error_code.val == gpik_res.error_code.SUCCESS)
			{
				for (unsigned int i = 0; i < gpik_res.solution.joint_state.name.size(); i++)
				{
					ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
				}
				joint_state_ = gpik_res.solution.joint_state;
			}
			else
			{
				ROS_ERROR("Inverse kinematics failed");
				return false;
			}
		}
		else
		{
			ROS_ERROR("Inverse kinematics service call failed");
			return false;
		}

		return true;
	}

	bool publishJoinStates()
	{
		joint_state_publisher_.publish(joint_state_);
		return true;
	}

private:

	ros::NodeHandle root_handle_;

	ros::Publisher joint_state_publisher_;
	ros::ServiceClient query_client;
	ros::ServiceClient ik_client;

	// define the service messages
	kinematics_msgs::GetKinematicSolverInfo::Request request;
	kinematics_msgs::GetKinematicSolverInfo::Response response;

	// define the service messages
	kinematics_msgs::GetPositionIK::Request gpik_req;
	kinematics_msgs::GetPositionIK::Response gpik_res;

	sensor_msgs::JointState joint_state_;
};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "get_ik_circle");
	ros::NodeHandle node_handle("~");

	double radius_;
	double a = 0.265;
	double px = 0, py, pz;
	double theta, roll;

	node_handle.param("radius", radius_, 0.868);

	GetIKCircle get_ik_circle;

	ros::Rate pub_rate(5.0);
	unsigned int i = 0;
	while(ros::ok())
	{
		if(i <= 200)
		{
			theta = M_PI/200*i;
			py = a + radius_ * sin(theta);
			pz = radius_ * cos(theta);
			roll = -theta;
			get_ik_circle.computeIK(px, py, pz, roll, 0.0, 0.0);
			get_ik_circle.publishJoinStates();
			++i;
			pub_rate.sleep();
		}
		else
		{
			i = 0;
			pub_rate.sleep();

		}


	}

	ros::shutdown();
	return 0;
}





